// Copyright (C) 2009  Yann de The
// 
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 2
// of the License, or (at your option) any later version.
// 
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
// 
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.

//012
//021
//102
//120
//201
//210

#include "SESpiceManager.h"
#define M_COL_X 0
#define M_COL_Y 1
#define M_COL_Z 2


namespace SESpice {
	void Init(const std::string& KerFile) {
		furnsh_c(KerFile.c_str());
	}
    
	Object::Object() : mName("[UNDEF]") {
		
	}
    
    Object::Object(const std::string& name) : mName(name) {
		
	}
	
	Object::~Object() {
	}
    
    void Object::getRadii(double* Req, double* Rpo) const {
        SpiceDouble radii[3];
        SpiceInt N;
        
        std::cout << "Appel" << std::endl;
        bodvrd_c(mName.c_str(), "RADII", 3, &N, radii);
        *Req = double(radii[0]);
        *Rpo = double(radii[2]);
//        std::cout << N << "," << *Req << "," << *Rpo << std::endl;
        
    }
    
	vec3d Object::getPosition(double t) const {
        vec3d mPosition;
        
		SpiceDouble tdb;
		SpiceDouble state[6];
		SpiceDouble lt;
		SpiceDouble xform[6][6];
		SpiceChar date[30];
		
		// Conversion de l'heure
        UTC_to_str(t, date);
		str2et_c(date, &tdb);
//        std::cout << tdb << std::endl;
		
		// Calcul pos. & vit.
		spkezr_c(mName.c_str(), tdb, "J2000", "LT+S", "Sun", state, &lt);
		mPosition = vec3d(state[0], state[1], state[2]);
		
        return mPosition;
	}
	
	vec3d Object::getVelocity(double t) const {
		vec3d mVelocity;
        
		SpiceDouble tdb;
		SpiceDouble state[6];
		SpiceDouble lt;
		SpiceDouble xform[6][6];
		SpiceChar date[30];
		
		// Conversion de l'heure
        UTC_to_str(t, date);
		str2et_c(date, &tdb);
		
		// Calcul pos. & vit.
		spkezr_c(mName.c_str(), tdb, "J2000", "LT+S", "Sun", state, &lt);
		mVelocity =vec3d(state[3], state[4], state[5]);
		
        return mVelocity;
	}
	
    mat4f Object::getAttitude(double t) const {
        mat4f transform;
        
		SpiceDouble tdb;
		SpiceChar date[30];
		SpiceDouble m_rot[3][3];
        SpiceDouble q[4];
        
		// Conversion de l'heure
		getPresentTime(date);
        UTC_to_str(t, date);
		str2et_c(date, &tdb);
		
		// Calcul attitude
//        void pxform_c(ConstSpiceChar* from, ConstSpiceChar* to, SpiceDouble et, SpiceDouble rotate[3][3] )
        pxform_c("J2000", (std::string("IAU_") + mName).c_str(), tdb, m_rot);
        transform(M_COL_X,0) = m_rot[0][0];
        transform(M_COL_X,1) = m_rot[0][1];
        transform(M_COL_X,2) = m_rot[0][2];
        transform(M_COL_X,3) = 0;
        
        transform(M_COL_Y,0) = m_rot[1][0];
        transform(M_COL_Y,1) = m_rot[1][1];
        transform(M_COL_Y,2) = m_rot[1][2];
        transform(M_COL_Y,3) = 0;
        
        transform(M_COL_Z,0) = m_rot[2][0];
        transform(M_COL_Z,1) = m_rot[2][1];
        transform(M_COL_Z,2) = m_rot[2][2];
        transform(M_COL_Z,3) = 0;
        
        transform(3,0) = 0;
        transform(3,1) = 0;
        transform(3,2) = 0;
        transform(3,3) = 1;
        
//        disp_mat4(transform);
        
        return transform;
	}
    
    mat4f Object::getTransform(double t) const {
        mat4f transform;
        
		SpiceDouble tdb;
		SpiceChar date[30];
		SpiceDouble m_rot[3][3];
        SpiceDouble q[4];
		SpiceDouble state[6];
		SpiceDouble lt;
        
		// Conversion de l'heure
		getPresentTime(date);
        UTC_to_str(t, date);
		str2et_c(date, &tdb);
		
		// Calcul attitude
        spkezr_c(mName.c_str(), tdb, "J2000", "LT+S", "Sun", state, &lt);
        pxform_c("J2000", (std::string("IAU_") + mName).c_str(), tdb, m_rot);
        transform(M_COL_X,0) = m_rot[0][0];
        transform(M_COL_X,1) = m_rot[0][1];
        transform(M_COL_X,2) = m_rot[0][2];
        transform(M_COL_X,3) = 0;
        
        transform(M_COL_Y,0) = m_rot[1][0];
        transform(M_COL_Y,1) = m_rot[1][1];
        transform(M_COL_Y,2) = m_rot[1][2];
        transform(M_COL_Y,3) = 0;
        
        transform(M_COL_Z,0) = m_rot[2][0];
        transform(M_COL_Z,1) = m_rot[2][1];
        transform(M_COL_Z,2) = m_rot[2][2];
        transform(M_COL_Z,3) = 0;
        
        transform(3,0) = state[0];
        transform(3,1) = state[1];
        transform(3,2) = state[2];
        transform(3,3) = 1;
        
        disp_mat4(transform);
        
        return transform;    }
	
	vec3d Object::getAngularVelocity(double t) const {
		vec3d mAngularVelocity = vec3d(0.,0.,0.);
        
		SpiceDouble tdb;
		SpiceDouble xform[6][6];
		SpiceChar date[30];
		
		// Conversion de l'heure
        UTC_to_str(t, date);
//		getPresentTime(date);
		str2et_c(date, &tdb);
		
		// Calcul att. & vit. angulaire
		sxform_c((std::string("IAU_") + mName).c_str(), "J2000", tdb, xform);
		
        for (int k=0; k<3; k++) {
            mAngularVelocity = mAngularVelocity + vec3d(-xform[4][k]*xform[5][k], xform[3][k]*xform[5][k], -xform[3][k]*xform[4][k]);
        }
        
        return mAngularVelocity;
	}
    
    std::string Object::getName() const {
        return mName;
    }
    
    SpiceDouble Object::getGravitaionalParameter() const {
        SpiceDouble GM;
        SpiceInt N;
        
        bodvrd_c(mName.c_str(), "GM", 1, &N, &GM);
        
        return GM;
        
    }
	
	void Clear(const std::string& KerFile) {
		unload_c(KerFile.c_str());
	}
}
